#include "modules/drivers/lidar/orin_hesai/hesai_utils/hesai_sdk.h"

#include <time.h>

#include "cyber/cyber.h"

namespace apollo {
namespace drivers {
namespace lidar_hesai {

bool HesaiSDK::Init(const uint32_t firing_data_port) {
  input_ = std::make_shared<SocketInput>();
  if (input_ == nullptr) {
    AERROR << "Failed to create socket input!";
    return false;
  }
  if (!input_->Init(firing_data_port, UDP_DATA_SIZE_WITH_SEQUENCE)) {
    AERROR << "Failed to initialize socket input!";
    return false;
  }
  return true;
}

bool HesaiSDK::Init(::apollo::drivers::lidar_hesai::Config &config) {
  input_ = std::make_shared<SocketInput>();
  if (input_ == nullptr) {
    AERROR << "Failed to create socket input!";
    return false;
  }
  if (!input_->Init(config.firing_data_port(), UDP_DATA_SIZE_WITH_SEQUENCE)) {
    AERROR << "Failed to initialize socket input!";
    return false;
  }
  return true;
}

void HesaiSDK::SetDataCallBack(HesaiLidarDataCallBack cb) { cb_ = cb; }

void HesaiSDK::Start() {
  while (!cyber::IsShutdown()) {
    if (GetPacket()) {
      if (cb_) {
        cb_(&packet_buffer_);
      } else {
        AERROR << "Hesai data callback function not set!";
      }
    } else {
      AWARN << "device poll failed";
      std::this_thread::sleep_for(std::chrono::nanoseconds(1000000));
    }
  }
  AERROR << "CompVelodyneDriver thread exit";
}

void HesaiSDK::CheckHighTemperature(const HesaiLidarPacketData &buf) {
  int high_temperature_index = TAIL_IDX + 9;
  if (buf[high_temperature_index] == 0x01) {
    AWARN << "Hesai LiDAR is in high temperature!";
  }
}

bool HesaiSDK::GetPacket() {
  int rc = input_->GetFiringDataPacket(&(packet_buffer_.hesaiPacket));
  if (rc == 0) return true;
  return false;
}

uint64_t HesaiSDK::GetPacketTimestamp(const HesaiLidarPacketData &buf) {
  int index = TIME_IDX;
  uint8_t utc_time[6];
  utc_time[0] = buf[index] & 0xff;
  utc_time[1] = buf[index + 1] & 0xff;
  utc_time[2] = buf[index + 2] & 0xff;
  utc_time[3] = buf[index + 3] & 0xff;
  utc_time[4] = buf[index + 4] & 0xff;
  utc_time[5] = buf[index + 5] & 0xff;
  uint64_t timestamp = (buf[index + 6] & 0xff) | (buf[index + 7] & 0xff) << 8 |
                       (buf[index + 8] & 0xff) << 16 |
                       (buf[index + 9] & 0xff) << 24;
  timestamp *= 1000;  // us to ns
  struct tm tTm;
  tTm.tm_year = utc_time[0] + 100;
  if (tTm.tm_year >= 200) {
    tTm.tm_year -= 100;
  }
  tTm.tm_mon = utc_time[1] - 1;
  tTm.tm_mday = utc_time[2];
  tTm.tm_hour = utc_time[3];
  tTm.tm_min = utc_time[4];
  tTm.tm_sec = utc_time[5];
  tTm.tm_isdst = 0;
  double unix_second = static_cast<double>(make_time_->mktime_kernel(&tTm));
  timestamp += static_cast<uint64_t>(unix_second) * 1e9;
  return timestamp;
}

}  // namespace lidar_hesai
}  // namespace drivers
}  // namespace apollo